#include "paths_routing.h"

Paths_Routing::Paths_Routing()
{
    num_next = ceil((paths_rou_par.Maxt * paths_rou_par.target_speed) / 0.3) + 12;    //局部片段搜索长度，根据目标速度和搜索时间确定，保证拟合曲线包括整个路径

    global_paths_receive_ = nh.subscribe("/xyl/refer_Point", 10, &Paths_Routing::global_paths, this);
    odom_ = nh.subscribe("/odom", 10, &Paths_Routing::Odom, this);
    laser_ = nh.subscribe("/scan", 1000, &Paths_Routing::laserCallback, this);
    GoalPose_ = nh.subscribe("/move_base_simple/goal", 10, &Paths_Routing::GoalPoseCallback, this); 
    Start_Dynamic_pub_ = nh.advertise<std_msgs::String>("/xyl/planning/Start_Dynamic", 10);           // 发布局部轨迹成功生存的信号
    vehicle_outline_pub_ = nh.advertise<visualization_msgs::Marker>("/vehicle_outline", 10);
    local_path_pub_ = nh.advertise<nav_msgs::Path>("/xyl/local_paths", 10);
    local_paths_a = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_paths_acceleration", 10);
    local_paths_v = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_paths_velocity", 10);
    local_paths_t = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_paths_theta", 10);
    local_paths_k = nh.advertise<geometry_msgs::PoseArray>("/xyl/local_paths_kappas", 10);

    string aa = "0";
    start_dynamic.data = aa.c_str();
    Start_Dynamic_pub_.publish(start_dynamic);
    paths_routing_thread_ = new boost::thread(boost::bind(&Paths_Routing::paths_routing_thread, this));
}

Paths_Routing::~Paths_Routing(){
    delete paths_routing_thread_;
    delete best_frenet_path;
}

void Paths_Routing::paths_routing_thread(void){

    FrenetHyperparameters fot_hp = {
        paths_rou_par.Max_speed,                                       // 最大速度 [m/s]，纵向和横向速度的尺量合
        paths_rou_par.Max_accel,                                        // 最大加速度[m/ss]
        paths_rou_par.Max_curvature,                              // 最大曲率 [1/m]
        paths_rou_par.left_d,                                                // 最大道路宽度 [m]
        paths_rou_par.right_d,                                             // 最小道路宽度 [m]
        0.0,                                                                                     //道路中间宽度，正好为0
        paths_rou_par.dDelta,                                              // 道路宽度采样间隔 [m]，值越小，轨迹好的越多，但是太小计算效率不行
        paths_rou_par.tDelta,                                               // 时间采样间隔[s]，值越大生成的轨迹速度越快
        paths_rou_par.Maxt,                                                 // 最大预测时间 [s]，值越大生成的轨迹距离越远
        paths_rou_par.Mint,                                                 // 最小预测时间 [s]，值越大生成的轨迹长度越长
        paths_rou_par.D_t_s,                                              // 目标速度采样间隔 [m/s]
        paths_rou_par.N_s_sample,                                // 目标速度的采样数量
        paths_rou_par.D_s,                                                 // 目标位置采样间隔 [m/s]
        paths_rou_par.N_s_sample,                               // 目标位置的采样数量

        // 损失函数权重
        5.0, // Distance from reference path
        5.0, // Target speed
        1.0, // Target acceleration
        0.5, // Jerk
        0.5, // time
        // 总的
        5.0, // Lateral
        1, // Longitudin
        
        /* //横向权重
        paths_rou_par.wlat_d, //横向偏差
        paths_rou_par.wlat_v, //横向速度
        paths_rou_par.wlat_a, //横向加速度
        paths_rou_par.wlat_j, //横向jerk（舒适性，即加速度导数）
        //纵向权重
        paths_rou_par.wlon_a, //纵向加速度
        paths_rou_par.wlon_j, //纵向jerk
        paths_rou_par.wlon_v, //纵向速度
        paths_rou_par.wlon_t, //纵向时间（也就路径越短越好）

        //总的
        paths_rou_par.wlat_score, // 横向权重得分
        paths_rou_par.wlon_score, //纵向权重得分 */
    };

    while (true)
    {
        if(is_path_segment == true && local_finish == false)
        {
            if (messege2 == true)
            {
                ds0 = best_ds0; // 初始的纵向速度[m/s]
            }
            best_frenet_path = nullptr;
            std::priority_queue<FrenetPath *, std::vector<FrenetPath *>, CostComparator_for_Frenet> frenet_paths;
            FrenetInitialConditions fot_ic = {
                init_lon_state,                                               // 初始的纵向值[m]
                ds0,                                                                    // 初始的纵向速度[m/s]
                dds0,                                                                 // 初始的纵向加速度[m/ss]
                d0,                                                                      // 初始的横向偏移值 [m]
                dd0,                                                                   // 初始的横向速度 [m/s]
                ddd0,                                                                // 初始的横向加速度 [m/s^2]
                0.0,                                                                    // 目标横向速度配置
                0.0,                                                                    // 目标横向加速度配置
                paths_rou_par.target_speed,               // 目标速度（即纵向的速度保持） [m/s]
                20,                                                                     // 目标位置,就是前探距离，测试3，按理说要变量赋值
                //   PathSegment.path_segment,        // 输入的参考线x坐标数组
            };
            frenet_opt_tra.calc_frenet_paths(&fot_ic, &fot_hp, frenet_paths, global_path_points, cache.accumulated_s, 
                                                                                                    PathSegment.vehicle_in_segment_index, Cub_spline_2D, s_d_point);

            // 筛选最优轨迹
            std::vector<FrenetPath*> valid_paths;
            double half_width = paths_rou_par.vehicle_width / 2;
            double threshold_levels[] = {half_width + 0.2 ,half_width + 0.15, half_width + 0.1};  // 定义不同的碰撞检测阈值
            int selected_path_index = 0; // 记录选中的路径索引
            double selected_threshold = 0; // 记录选中的阈值

            // 遍历优先队列中的路径并进行碰撞检测
            while (!frenet_paths.empty()) {
                auto top = frenet_paths.top();
                frenet_paths.pop();

                for (double threshold : threshold_levels) {
                    if (!is_collision(top, threshold)) {
                        valid_paths.push_back(top);  // 保存通过当前阈值的路径
                        if (selected_path_index == 0) { // 只记录第一个符合条件的路径
                            selected_path_index = valid_paths.size();
                            selected_threshold = threshold;
                        }
                        break;  // 如果当前路径满足某个阈值，跳出循环，不再检查更低阈值
                    }
                }
                // 如果路径没有被保存，意味着它未通过任何一个阈值的检测，需要删除它
                if (std::find(valid_paths.begin(), valid_paths.end(), top) == valid_paths.end()) {
                    delete top;
                }
            }

            // 优先选择满足较高阈值的路径
            for (double threshold : threshold_levels) {
                for (auto path : valid_paths) {
                    if (!is_collision(path, threshold)) {
                        best_frenet_path = path;
                        break;
                    }
                }
                if (best_frenet_path != nullptr) {
                    break;  // 如果找到了合适的路径，立即跳出
                }
            }

            // 清理未选中的路径
            for (auto path : valid_paths) {
                if (path != best_frenet_path) {
                    delete path;
                }
            }

            messege2 = false;
            if (best_frenet_path->x.size() == 0 || best_frenet_path == nullptr) // 没有轨迹数量
            {
                if (messege2 == false) // 只发布一次
                {
                    ROS_WARN("No valid trajectory found!");
                    string aa = "2";
                    start_dynamic.data = aa.c_str();
                    Start_Dynamic_pub_.publish(start_dynamic);
                    traj_points_.poses.clear();
                    traj_points_.header.frame_id = "map";
                    traj_points_.header.stamp = ros::Time::now();
                    local_path_pub_.publish(traj_points_);
                    messege1 = false; // 复位
                    messege2 = true;  // 复位
                }
            }
            else
            {
                //---------------------------------发布轨迹---------------------------------//
                // 复制给traj_points_，给发布
                traj_points_.poses.clear();
                traj_points_.header.frame_id = "map";
                traj_points_.header.stamp = ros::Time::now();
                for (int i = 0; i < best_frenet_path->x.size(); i++)
                {
                    geometry_msgs::PoseStamped pose_stamp;
                    pose_stamp.header.frame_id = "map";
                    pose_stamp.header.stamp = ros::Time::now();
                    pose_stamp.pose.position.x = best_frenet_path->x[i];
                    pose_stamp.pose.position.y =  best_frenet_path->y[i];
                    pose_stamp.pose.position.z = 0;
                    traj_points_.poses.push_back(pose_stamp);
                }
                local_path_pub_.publish(traj_points_);

                // 发布速度
                pubLocalPath_v.poses.clear();
                pubLocalPath_v.header.frame_id = "map";
                pubLocalPath_v.header.stamp = ros::Time::now();
                for (size_t i = 0; i < best_frenet_path->v.size(); i++)
                {
                    geometry_msgs::Pose init_v;
                    init_v.position.x = best_frenet_path->v[i];
                    pubLocalPath_v.poses.push_back(init_v);
                }
                local_paths_v.publish(pubLocalPath_v);
                //--------------------------------发布加速度---------------------------------//
                pubLocalPath_a.poses.clear();
                pubLocalPath_a.header.frame_id = "map";
                pubLocalPath_a.header.stamp = ros::Time::now();
                for (size_t i = 0; i <  best_frenet_path->a.size(); i++)
                {
                    geometry_msgs::Pose init_pose;
                    init_pose.position.x = best_frenet_path->a[i];
                    pubLocalPath_a.poses.push_back(init_pose);
                }
                local_paths_a.publish(pubLocalPath_a);
                //--------------------------------发布角度---------------------------------//
                pubLocalPath_t.poses.clear();
                pubLocalPath_t.header.frame_id = "map";
                pubLocalPath_t.header.stamp = ros::Time::now();
                // int uuu = 0;
                for (size_t i = 0; i < best_frenet_path->theta.size(); i++)
                {
                    geometry_msgs::Pose init_pose;
                    init_pose.position.x = best_frenet_path->theta[i];
                    pubLocalPath_t.poses.push_back(init_pose);
                }
                local_paths_t.publish(pubLocalPath_t);
                //--------------------------------发布曲率---------------------------------//
                pubLocalPath_k.poses.clear();
                pubLocalPath_k.header.frame_id = "map";
                pubLocalPath_k.header.stamp = ros::Time::now();
                for (size_t i = 0; i <  best_frenet_path->kappas.size(); i++)
                {
                    geometry_msgs::Pose init_pose;
                    init_pose.position.x =  best_frenet_path->kappas[i];
                    pubLocalPath_k.poses.push_back(init_pose);
                }
                local_paths_k.publish(pubLocalPath_k);

                if (messege1 == false) // 只发布一次,最开始生成局部轨迹的时候发
                {
                    sleep(0.3); // 等会再开车，延时1秒
                    string aa = "1";
                    start_dynamic.data = aa.c_str();
                    Start_Dynamic_pub_.publish(start_dynamic);
                    messege1 = true;
                }

                Point current_point;

                if (is_update_dynamic(traj_points_, odom, 12) == true)
                {
                    double points_s, points_d;
                    current_point.x = odom.pose.pose.position.x;
                    current_point.y = odom.pose.pose.position.y;
                    cft.cartesianToFrenet(global_path_points, current_point, points_s, points_d, projection_point.index);
                    init_lon_state = points_s;
                    ds0 = best_frenet_path->s_d[12];
                    best_ds0 =  best_frenet_path->s_d[12];
                    dds0 = best_frenet_path->s_dd[12];
                    d0 = best_frenet_path->d[12];
                    /* dd0 = best_frenet_path->d_d[12];
                    ddd0 = best_frenet_path->d_dd[12]; */
                }
            }
            double goal_distance = std::hypot(odom.pose.pose.position.x - goal_pose_.position.x, odom.pose.pose.position.y - goal_pose_.position.y);
            if (goal_distance < 0.3)
            {
                ROS_INFO("Successfully reached the target point.");
                local_finish = true;
                is_path_segment = false;
            }

        }
    }
}

bool Paths_Routing::is_update_dynamic(nav_msgs::Path &trj_point_array, nav_msgs::Odometry &odom, int size)
{
    double distance = std::sqrt(std::pow(odom.pose.pose.position.x - trj_point_array.poses[size].pose.position.x, 2) +
                            pow(odom.pose.pose.position.y - trj_point_array.poses[size].pose.position.y, 2));
    if (distance < 1) // 接近了
    {
        return true;
    }
    return false;
}

bool Paths_Routing::is_collision(FrenetPath *fts, double COLLISION_CHECK) {
    // 获取障碍物点
    const size_t step_size = 3;  // 每隔若干点检查一次，减少计算量
    std::vector<myObstacle> obstacles_ptr = obs.getObstaclesPoints();

    // 预先计算碰撞检查的平方值，避免每次调用 hypot
    double collision_check_squared = COLLISION_CHECK * COLLISION_CHECK;

    // 遍历障碍物点
    for (const auto& obstacle_ptr : obstacles_ptr) {
        // 初步过滤：快速检测是否有可能碰撞
        bool potential_collision = false;
        for (size_t j = 0; j < fts->x.size(); j += step_size) {
            double dx = obstacle_ptr.x - fts->x[j];
            double dy = obstacle_ptr.y - fts->y[j];
            double distance_squared = dx * dx + dy * dy;

            // 如果距离小于初步碰撞范围，则标记为潜在碰撞
            if (distance_squared <= (collision_check_squared + 0.1 * 0.1)) {
                potential_collision = true;
                break;
            }
        }

        // 如果没有潜在碰撞，跳过这个障碍物
        if (!potential_collision) continue;

        // 精确碰撞检测（逐点检查）
        for (size_t j = 0; j < fts->x.size(); j += step_size) {
            double front_x = fts->x[j];
            double front_y = fts->y[j];
            double yaw = fts->theta[j];

            // 计算车辆前后两个点的位置
            double rear_x = front_x - paths_rou_par.vehicle_length * cos(yaw);
            double rear_y = front_y - paths_rou_par.vehicle_length * sin(yaw);

            // 定义车辆的关键点（前点和后点）
            std::vector<std::pair<double, double>> vehicle_points{
                {front_x, front_y},  // 前点
                {rear_x, rear_y}     // 后点
            };

            // 检查每个关键点是否与障碍物发生碰撞
            for (const auto& point : vehicle_points) {
                double dx = obstacle_ptr.x - point.first;
                double dy = obstacle_ptr.y - point.second;
                double distance_squared = dx * dx + dy * dy;

                if (distance_squared <= collision_check_squared) {
                    return true;  // 如果碰撞，立即返回
                }
            }
        }
    }

    // 如果所有障碍物都未发生碰撞，返回 false
    return false;
}

void Paths_Routing::GoalPoseCallback(const geometry_msgs::PoseStamped &msg)
{
    if(is_goal_receive == false){
        goal_pose_.position.x = msg.pose.position.x;
        goal_pose_.position.y = msg.pose.position.y;
        theta_end = tf2::getYaw(msg.pose.orientation);
        is_goal_receive = true;
    }
}

void Paths_Routing::global_paths(const nav_msgs::Path &path_point_){

    if (path_point_.poses.size() > 0){
        for (size_t i = 0; i < path_point_.poses.size(); i++){
            std::pair<double, double> xy;
            xy.first = path_point_.poses[i].pose.position.x;
            xy.second = path_point_.poses[i].pose.position.y;
            global_path_points.push_back(xy);
            // path_manager.AddPoint(xy);
        }
    }

}

bool Paths_Routing::paths_segment_intercept(int proj_index , CachedPathProfile &cache) {
    // 定义前后点数,截取片段
    int num_prev = 6;
    // 计算可能的起始和结束索引
    int start_index = std::max(0, proj_index - num_prev);
    int end_index = std::min(static_cast<int>(global_path_points.size()) - 1, proj_index + num_next);
    // 如果前面的点不够，后面补
    if (proj_index - start_index < num_prev) {
        int extra_needed = num_prev - (proj_index - start_index);
        end_index = std::min(static_cast<int>(global_path_points.size()) - 1, end_index + extra_needed);
    }
    // 如果后面的点不够，前面补
    if (end_index - proj_index < num_next) {
        int extra_needed = num_next - (end_index - proj_index);
        start_index = std::max(0, start_index - extra_needed);
    }
    // 清空之前的片段
    PathSegment.path_segment.clear();
    PathSegment.accumulated_s.clear();
    PathSegment.dkappas.clear();
    PathSegment.headings.clear();
    PathSegment.kappas.clear();
    // 提取子路径并记录车辆点索引
    for (int i = start_index; i <= end_index; ++i) {
        PathSegment.path_segment.push_back(global_path_points[i]);
        PathSegment.accumulated_s.push_back(cache.accumulated_s[i]);
        PathSegment.dkappas.push_back(cache.dkappas[i]);
        PathSegment.headings.push_back(cache.headings[i]);
        PathSegment.kappas.push_back(cache.kappas[i]);
    }
    PathSegment.vehicle_in_segment_start_index = start_index;
    PathSegment.vehicle_in_segment_index = proj_index; // 保存车辆点在片段中的索引

    // auto CubicSpline2D_start = std::chrono::steady_clock::now();
    Cub_spline_2D = std::make_unique<CubicSpline2D>(PathSegment.path_segment, PathSegment.accumulated_s, 
                                                        PathSegment.vehicle_in_segment_start_index, PathSegment.vehicle_in_segment_index);
    /* auto CubicSpline2D_end = std::chrono::steady_clock::now();
    double CubicSpline2D_millsecond = std::chrono::duration<double, std::milli>(CubicSpline2D_end - CubicSpline2D_start).count();
    std::cout << CubicSpline2D_millsecond << "ms" << std::endl; */

    return !PathSegment.path_segment.empty();
}

void Paths_Routing::Odom(const nav_msgs::Odometry &odom_msg_){
    is_path_segment = false;
    odom = odom_msg_;
    // auto path_select_start = std::chrono::steady_clock::now();
    vehicle_outline();
    std::pair<double, double> odom_receive_;
    odom_receive_.first = odom_msg_.pose.pose.position.x;
    odom_receive_.second = odom_msg_.pose.pose.position.y;
    if (!global_path_points.empty() && is_frist_odom == false)
    {
        double start_s, start_d;
        Point odom_point;
        odom_point.x = odom.pose.pose.position.x;
        odom_point.y = odom.pose.pose.position.y;
        cft.cartesian_Frenet(global_path_points,  odom_point, start_s, start_d);
        d0 = start_d;                                                              // 初始的横向偏移值 [m]
        dd0 = 0;                                                                       // 初始的横向速度 [m/s]
        ddd0 = 0;                                                                    // 初始的横向加速度 [m/s^2]
        init_lon_state = start_s;                                      // 初始的纵向值[m]
        ds0 = paths_rou_par.initial_vs;                       // 初始的纵向速度[m/s]
        dds0 = 0;                                                                    // 初始的纵向加速度[m/ss]
        // std::cout << "init_lon_state: " << init_lon_state << "          d0: " << d0 << std::endl;
        is_frist_odom = true;
        messege1 = false;
    }
    
    if (!global_path_points.empty() && is_frist_odom == true) {
        projection_point = match_pp.match_and_projection_point(odom_receive_ , global_path_points);
        is_match_projection = true;
        if(is_global_s_heading_ == true && !match_pp.ComputePathProfileWithCache(global_path_points, cache)){
            is_global_s_heading_ = false;
            is_global_s_heading_success = true;
        }
        is_path_segment = paths_segment_intercept(projection_point.index, cache);
    }

    /* auto path_select_end = std::chrono::steady_clock::now();
    double path_select = std::chrono::duration<double, std::milli>(path_select_end - path_select_start).count();
    std::cout << path_select << "ms" << std::endl; */

}

void Paths_Routing::laserCallback(const sensor_msgs::LaserScan& scan) {
    s_d_point.clear();
    sensor_msgs::LaserScan scan_receive_ = scan;
    if(is_match_projection == true){
        obs.laser( scan_receive_, odom, global_path_points);
        // obstacles_projections = obs.getObstaclesProjections();
    }

    std::vector<myObstacle> obstacles_points = obs.getObstaclesPoints();
    for(size_t i = 0; i < obstacles_points.size(); i++){
        double ctf_s;
        double ctf_d;
        point.x = obstacles_points[i].x;
        point.y = obstacles_points[i].y;
        cft.cartesianToFrenet(global_path_points,  point, ctf_s, ctf_d, projection_point.index);
        s_d_point.push_back(std::make_pair(ctf_s, ctf_d));
    }
    obs.visualizeObstacles(obstacles_points);
}

void Paths_Routing::vehicle_outline(){
    double x = odom.pose.pose.position.x;
    double y = odom.pose.pose.position.y;
    double yaw = tf::getYaw(odom.pose.pose.orientation);
    double half_width = paths_rou_par.vehicle_width / 2;
    double half_length = paths_rou_par.vehicle_length / 2;

    // 计算四个顶点
    std::vector<geometry_msgs::Point> corners;

    for (auto &corner : std::vector<std::pair<double, double>>{
            {half_length, half_width},
            {half_length, -half_width},
            {-half_length, -half_width},
            {-half_length, half_width}}) {
    geometry_msgs::Point p;
    p.x = x + corner.first * cos(yaw) - corner.second * sin(yaw);
    p.y = y + corner.first * sin(yaw) + corner.second * cos(yaw);
    p.z = 0;
    corners.push_back(p);
    }

    // 创建Marker
    visualization_msgs::Marker marker_outline;
    marker_outline.header.frame_id = "map";
    marker_outline.header.stamp = ros::Time::now();
    marker_outline.ns = "vehicle";
    marker_outline.id = 0;
    marker_outline.type = visualization_msgs::Marker::LINE_STRIP;
    marker_outline.action = visualization_msgs::Marker::ADD;
    marker_outline.scale.x = 0.1; // 线条宽度

    // 车辆轮廓颜色
    marker_outline.color.r = 1.0;
    marker_outline.color.g = 0.0;
    marker_outline.color.b = 0.0;
    marker_outline.color.a = 1.0;
    // 添加顶点
    for (const auto &corner : corners) {
    marker_outline.points.push_back(corner);
    }
    // 连接最后一个点到第一个点
    marker_outline.points.push_back(corners.front());

    vehicle_outline_pub_.publish(marker_outline);
}
